#include "nucCommu.h"
#include "CAN_receive.h"
#include "pid.h"
#include "cmsis_os.h"
#include "robot_total_mode.h"
#include "arm_math.h"
#include "OLED.h"
#include "user_lib.h"
#include "detect_task.h"
#include "referee.h"

//********************建系方案**************************//
//前进方向为x正方向，竖直向上为w（角速度）正方向，Y=W×X(正方向)
//电机左前等指面向x正方向的左前【顺时针一圈】
//GM6020的中值指轮向前且M3508在内测
//以下规定只在解算时起作用，发送指令时根据实际情况来取符号
//M3508的转动方向取正指GM6020在中值时向前运动的转动方向
//GM6020的转动方向取正为竖直向上【角度取值范围[-90,90]】【和实际相同】
//****************************机器人控制常量***************//
#define MAX_SPEED 1000.0f //最大速度
#define MAX_OMEGA 2.0f       //最大角速度

#define ECD_FULL_ROUND 8192 //一圈的ECD值。实际取值0-8191
// TODO: 调车时重新测量中心ECD，放在此文件中
//预先测量的云台在中心时的yawECD。在机器人重新安装云台后，可能导致ECD和这个不一样
#define YAW_MID_ECD 684
#define CHASSIS_LF_G_MID_ECD 2717
#define CHASSIS_RF_G_MID_ECD 7541
#define CHASSIS_RB_G_MID_ECD 7529
#define CHASSIS_LB_G_MID_ECD 7473
#define WHEEL_PERIMETER 114.0f*PI //单位为mm
#define M3508_K (3591.0f/187.0f) //3508减速比
#define INT8_MAX 127.0f
#define INT16_MAX 32767.0f

#define CHASSIS_R 460.83f //单位为mm
#define CHASSIS_THETA_SIN 0.7071f
#define CHASSIS_THETA_COS 0.7071f

//***************************PID控制参数*****************//
#define M3508_MOTOR_SPEED_PID_KP 20.0f
#define M3508_MOTOR_SPEED_PID_KI 5.0f
#define M3508_MOTOR_SPEED_PID_KD 0.0f
#define M3508_MOTOR_SPEED_PID_MAX_OUT 15000.0f
#define M3508_MOTOR_SPEED_PID_MAX_IOUT 4000.0f

#define GM_1_6020_CHESSIS_AGL_KP 100.0f
#define GM_1_6020_CHESSIS_AGL_KI 0.0f
#define GM_1_6020_CHESSIS_AGL_KD 0.0f
#define GM_1_6020_CHESSIS_AGL_MAXOUT 50.0f
#define GM_1_6020_CHESSIS_AGL_MAX_IOUT 0.7f

#define GM_2_6020_CHESSIS_AGL_KP 100.0f
#define GM_2_6020_CHESSIS_AGL_KI 0.0f
#define GM_2_6020_CHESSIS_AGL_KD 0.0f
#define GM_2_6020_CHESSIS_AGL_MAXOUT 50.0f
#define GM_2_6020_CHESSIS_AGL_MAX_IOUT 0.7f

#define GM_3_6020_CHESSIS_AGL_KP 120.0f
#define GM_3_6020_CHESSIS_AGL_KI 0.0f
#define GM_3_6020_CHESSIS_AGL_KD 0.0f
#define GM_3_6020_CHESSIS_AGL_MAXOUT 50.0f
#define GM_3_6020_CHESSIS_AGL_MAX_IOUT 0.7f

#define GM_4_6020_CHESSIS_AGL_KP 100.0f
#define GM_4_6020_CHESSIS_AGL_KI 0.0f
#define GM_4_6020_CHESSIS_AGL_KD 0.0f
#define GM_4_6020_CHESSIS_AGL_MAXOUT 50.0f
#define GM_4_6020_CHESSIS_AGL_MAX_IOUT 0.7f



#define GM_1_6020_CHESSIS_SPD_KP 240.0f
#define GM_1_6020_CHESSIS_SPD_KI 0.0f
#define GM_1_6020_CHESSIS_SPD_KD 0.0f
#define GM_1_6020_CHESSIS_SPD_MAXOUT 15000.0f
#define GM_1_6020_CHESSIS_SPD_MAX_IOUT 2000.0f

#define GM_2_6020_CHESSIS_SPD_KP 110.0f
#define GM_2_6020_CHESSIS_SPD_KI 0.0f
#define GM_2_6020_CHESSIS_SPD_KD 0.0f
#define GM_2_6020_CHESSIS_SPD_MAXOUT 15000.0f
#define GM_2_6020_CHESSIS_SPD_MAX_IOUT 2000.

#define GM_3_6020_CHESSIS_SPD_KP 130.0f
#define GM_3_6020_CHESSIS_SPD_KI 0.0f
#define GM_3_6020_CHESSIS_SPD_KD 0.0f
#define GM_3_6020_CHESSIS_SPD_MAXOUT 15000.0f
#define GM_3_6020_CHESSIS_SPD_MAX_IOUT 2000.0f

#define GM_4_6020_CHESSIS_SPD_KP 90.0f
#define GM_4_6020_CHESSIS_SPD_KI 0.0f
#define GM_4_6020_CHESSIS_SPD_KD 0.0f
#define GM_4_6020_CHESSIS_SPD_MAXOUT 15000.0f
#define GM_4_6020_CHESSIS_SPD_MAX_IOUT 2000.0f



#define MAX_WHEEL_SPEED 4.0f    //单个底盘电机最大速度
#define NORMAL_MAX_CHASSIS_SPEED_X 2.0f //底盘运动过程最大前进速度
#define NORMAL_MAX_CHASSIS_SPEED_Y 1.5f //底盘运动过程最大平移速度
#define NORMAL_MAX_CHASSIS_SPEED_W 0.9f

//*************************任务相关常量*******************//

#define CHASSIS_TASK_INIT_TIME 500      //电机初始化时间
#define CHASSIS_CONTROL_TIME_MS 5       //每5ms执行一次任务

#define rc_deadband_limit(input, output, dealine)        \
    {                                                    \
        if ((input) > (dealine) || (input) < -(dealine)) \
        {                                                \
            (output) = (input);                          \
        }                                                \
        else                                             \
        {                                                \
            (output) = 0;                                \
        }                                                \
    }

//******************数字滤波所需常量****************// 系数和控制方法来自官方步兵
#define CHASSIS_ACCEL_X_NUM 150.1666f
#define CHASSIS_ACCEL_Y_NUM 150.1666f
#define CHASSIS_ACCEL_W_NUM 150.66f


//******************功率控制相关常量***********************//   // 2.3
#define FULL_BUFFER         60.0f   // 最大缓冲能量
#define DANGEROUS_BUFFER    5.0f    // 危险缓冲能量，绝对不能让缓冲能量低于这个值

#define CURRENT_TO_DATA     819.2f  // 每A电流对应发送的数据。物理的安培对应的CAN数据
#define VOLTAGE             24.0f   // 供电电压为24V

#define COMMUNICATING_PERIOD_S  0.1f     // 与裁判系统的通讯周期。单位为s

// 用于调试具体功率
// #define TEST_P40
#define TEST_P60
#define TEST_P60
// #define TEST_P80
// #define TEST_P100
// #define TEST_P45
// #define TEST_P50
// #define TEST_P55

// #define NO_BUFFER   // 2.18，无buffer状态，用于调试抬升电流大小

#define abss(input, output)     \
{                               \
    if ((input) >= 0)             \
        (output) = (input);         \
    else                        \
        (output) = -(input);        \
}


//限幅函数
#define range_limit_inside(toBeLimited, range)                      \
    {                                                               \
         if((toBeLimited)>(range))                                  \
        {                                                           \
            (toBeLimited)=(range);                                  \
        }                                                           \
        else if((toBeLimited)<-(range))                             \
        {                                                           \
            (toBeLimited)=-(range);                                 \
        }                                                           \
    }
//**************************控制量结构体*******************//


//机器人控制量
struct RobotControl_s{
    fp32 vx,vy,w;
    first_order_filter_type_t vx_filter,vy_filter,w_filter;
};

//云台yaw电机相对于底盘角度结构体
struct Angle_s{
    int16_t nowGimbalYawECD;        //目前的YAW轴云台ECD
    fp32    gimbalAngleFromChassis; //云台相对于最初的角度，逆时针为正方向。单位弧度。
    int16_t nowChessAGVECD[4];        //目前的四个舵轮GM6030ECD
};

struct AGVControl_t
{
    fp32 vx;                    //x方向速度
    fp32 vy;                    //y方向速度
    fp32 presentAngle;          //当前角度
    fp32 wantedAngle;           //期望角度
    fp32 presentMotorSpeed;     //当前速度
    fp32 presentMotorSpeed_GM;     //GM6020当前速度
    fp32 wantedMotorSpeed;      //期望速度
    int8_t directionFlag;       //directionFlag*vx=实际vx
};


//底盘M3508控制
struct MotorControl_M_t{
    pid_type_def    vpid;
    int16_t         giveCurrent;
};

//底盘GM6020控制
struct MotorControl_GM_t{
    pid_type_def    vpidOfRpm;
    pid_type_def    vpidOfVolage;
    int16_t         giveCurrent;
};


//************************用于控制的全局变量**********//
static int zeroCurrentMark;    //当离线时，直接发送零电流的时候
static struct Angle_s someAngle;   //用于记录所有的云台、底盘角度相关值
static struct MotorControl_M_t driveMotor_M[4]; //M3508
static struct MotorControl_GM_t driveMotor_GM[4]; //GM6020
static struct AGVControl_t AGVControl[4]; //舵轮控制
static const fromGimbalSTM_t *gimbalSTM_p;   //遥控器位置指针，需要初始化
static const toSTM32_t *nuc_p;  //NUC数据位置。未来制造全自动机器人时需要
const static fp32 motor_M_speed_pid[3] = {M3508_MOTOR_SPEED_PID_KP, M3508_MOTOR_SPEED_PID_KI, M3508_MOTOR_SPEED_PID_KD}; //PID 初始化参数
const static fp32 motor_GM_1_ang_pid[3] = {GM_1_6020_CHESSIS_AGL_KP,GM_1_6020_CHESSIS_AGL_KI,GM_1_6020_CHESSIS_AGL_KD}; //GM6020速度环PID 初始化参数
const static fp32 motor_GM_2_ang_pid[3] = {GM_2_6020_CHESSIS_AGL_KP,GM_2_6020_CHESSIS_AGL_KI,GM_2_6020_CHESSIS_AGL_KD}; //GM6020速度环PID 初始化参数
const static fp32 motor_GM_3_ang_pid[3] = {GM_3_6020_CHESSIS_AGL_KP,GM_3_6020_CHESSIS_AGL_KI,GM_3_6020_CHESSIS_AGL_KD}; //GM6020速度环PID 初始化参数
const static fp32 motor_GM_4_ang_pid[3] = {GM_4_6020_CHESSIS_AGL_KP,GM_4_6020_CHESSIS_AGL_KI,GM_4_6020_CHESSIS_AGL_KD}; //GM6020速度环PID 初始化参数
const static fp32 motor_GM_1_speed_pid[3] = {GM_1_6020_CHESSIS_SPD_KP,GM_1_6020_CHESSIS_SPD_KI,GM_1_6020_CHESSIS_SPD_KD}; //GM6020电压环PID 初始化参数
const static fp32 motor_GM_2_speed_pid[3] = {GM_2_6020_CHESSIS_SPD_KP,GM_2_6020_CHESSIS_SPD_KI,GM_2_6020_CHESSIS_SPD_KD}; //GM6020电压环PID 初始化参数
const static fp32 motor_GM_3_speed_pid[3] = {GM_3_6020_CHESSIS_SPD_KP,GM_3_6020_CHESSIS_SPD_KI,GM_3_6020_CHESSIS_SPD_KD}; //GM6020电压环PID 初始化参数
const static fp32 motor_GM_4_speed_pid[3] = {GM_4_6020_CHESSIS_SPD_KP,GM_4_6020_CHESSIS_SPD_KI,GM_4_6020_CHESSIS_SPD_KD}; //GM6020电压环PID 初始化参数
static const enum RobotState_e *robotMode;   //机器人模式
static struct RobotControl_s robotTotalSpeedControl;
static int16_t vx_channel,vy_channel,w_channel; //遥控器deadband limit后输出的值


//************************各个功能函数*************//
void initPIDs(void)
{
    int i;
    for(i=0;i<4;i++)
    {
        PID_init(&(driveMotor_M[i].vpid),PID_POSITION,motor_M_speed_pid,M3508_MOTOR_SPEED_PID_MAX_OUT,M3508_MOTOR_SPEED_PID_MAX_IOUT);
        if(i==0)
        {
        PID_init(&(driveMotor_GM[i].vpidOfRpm),PID_POSITION,motor_GM_1_ang_pid,GM_1_6020_CHESSIS_AGL_MAXOUT,GM_1_6020_CHESSIS_AGL_MAX_IOUT);
        PID_init(&(driveMotor_GM[i].vpidOfVolage),PID_POSITION,motor_GM_1_speed_pid,GM_1_6020_CHESSIS_SPD_MAXOUT,GM_1_6020_CHESSIS_SPD_MAX_IOUT);
        }
        if(i==1)
        {
        PID_init(&(driveMotor_GM[i].vpidOfRpm),PID_POSITION,motor_GM_2_ang_pid,GM_2_6020_CHESSIS_AGL_MAXOUT,GM_2_6020_CHESSIS_AGL_MAX_IOUT);
        PID_init(&(driveMotor_GM[i].vpidOfVolage),PID_POSITION,motor_GM_2_speed_pid,GM_2_6020_CHESSIS_SPD_MAXOUT,GM_2_6020_CHESSIS_SPD_MAX_IOUT);
        }
        if(i==2)
        {
        PID_init(&(driveMotor_GM[i].vpidOfRpm),PID_POSITION,motor_GM_3_ang_pid,GM_3_6020_CHESSIS_AGL_MAXOUT,GM_3_6020_CHESSIS_AGL_MAX_IOUT);
        PID_init(&(driveMotor_GM[i].vpidOfVolage),PID_POSITION,motor_GM_3_speed_pid,GM_3_6020_CHESSIS_SPD_MAXOUT,GM_3_6020_CHESSIS_SPD_MAX_IOUT);
        }
        if(i==3)
        {
        PID_init(&(driveMotor_GM[i].vpidOfRpm),PID_POSITION,motor_GM_4_ang_pid,GM_4_6020_CHESSIS_AGL_MAXOUT,GM_4_6020_CHESSIS_AGL_MAX_IOUT);
        PID_init(&(driveMotor_GM[i].vpidOfVolage),PID_POSITION,motor_GM_4_speed_pid,GM_4_6020_CHESSIS_SPD_MAXOUT,GM_4_6020_CHESSIS_SPD_MAX_IOUT);
        }
    }
}

void initFilters(void)
{
    const static fp32 chassis_x_order_filter[1] = {CHASSIS_ACCEL_X_NUM};
    const static fp32 chassis_y_order_filter[1] = {CHASSIS_ACCEL_Y_NUM};
    const static fp32 chassis_w_order_filter[1] = {CHASSIS_ACCEL_W_NUM};
    
    first_order_filter_init(&(robotTotalSpeedControl.vx_filter), CHASSIS_CONTROL_TIME_MS, chassis_x_order_filter);
    first_order_filter_init(&(robotTotalSpeedControl.vy_filter), CHASSIS_CONTROL_TIME_MS, chassis_y_order_filter);
    first_order_filter_init(&(robotTotalSpeedControl.w_filter), CHASSIS_CONTROL_TIME_MS, chassis_w_order_filter);

}

void setChassisMode(void)
{
    zeroCurrentMark=0;
    if(RobotState_e_Powerless== *robotMode)
        zeroCurrentMark=1;
    else if(RobotState_e_Brilliant == *robotMode)
    {
        if(game_not_started())
            zeroCurrentMark = 1;
    }
}

static void analyseTotalControl(void)    //通过遥控器和键盘参数设置速度。当键盘输出量为0时，通过遥控器摇杆确定速度。
{
    robotTotalSpeedControl.vx=0;robotTotalSpeedControl.vy=0;robotTotalSpeedControl.w=0;
    
    if(RobotState_e_Powerless==*robotMode)
    {
        return;
    }
    else if(RobotState_e_Brilliant==*robotMode)
    {
        robotTotalSpeedControl.vx = ((fp32)nuc_p->x)*MAX_SPEED/INT16_MAX;
        robotTotalSpeedControl.vy = ((fp32)nuc_p->y)*MAX_SPEED/INT16_MAX;
        robotTotalSpeedControl.w =  ((fp32)nuc_p->w)*MAX_OMEGA/INT16_MAX;       
    }else{
        robotTotalSpeedControl.vx = ((fp32)gimbalSTM_p->x)*MAX_SPEED/INT8_MAX;
        robotTotalSpeedControl.vy = ((fp32)gimbalSTM_p->y)*MAX_SPEED/INT8_MAX;
        robotTotalSpeedControl.w = ((fp32)gimbalSTM_p->w)*MAX_OMEGA/INT8_MAX;  
    }   
}

static void refreshECD(void)   //更新当前云台yawECD值和弧度制的值，实现云台坐标系控制以及小陀螺
{
    someAngle.nowGimbalYawECD=get_chassis_motor_measure_point(YAW_DOWN)->ecd;  
    someAngle.gimbalAngleFromChassis=(someAngle.nowGimbalYawECD - YAW_MID_ECD) * 2 * PI / ECD_FULL_ROUND;
    for(int i=0;i<4;i++){
        someAngle.nowChessAGVECD[i]=get_chassis_motor_measure_point(CHASSIS_LF_G+i)->ecd;
    }
    AGVControl[0].presentAngle=(someAngle.nowChessAGVECD[0]-CHASSIS_LF_G_MID_ECD)* 2 * PI / ECD_FULL_ROUND;
    AGVControl[1].presentAngle=(someAngle.nowChessAGVECD[1]-CHASSIS_RF_G_MID_ECD)* 2 * PI / ECD_FULL_ROUND;
    AGVControl[2].presentAngle=(someAngle.nowChessAGVECD[2]-CHASSIS_RB_G_MID_ECD)* 2 * PI / ECD_FULL_ROUND;
    AGVControl[3].presentAngle=(someAngle.nowChessAGVECD[3]-CHASSIS_LB_G_MID_ECD)* 2 * PI / ECD_FULL_ROUND;
}

static void calcWheelVelocityInChassisAxis(void)   //将控制量中的坐标系速度转化为底盘坐标系的速度，并计算轮子转动速度
{
    fp32 sin_yaw,cos_yaw;
    fp32 vx,vy,w;
    sin_yaw=arm_sin_f32(someAngle.gimbalAngleFromChassis);
    cos_yaw=arm_cos_f32(someAngle.gimbalAngleFromChassis);
    w=robotTotalSpeedControl.w;
    //将云台坐标系速度转化为底盘坐标系速度。坐标系变换
    vx=cos_yaw*robotTotalSpeedControl.vx-sin_yaw*robotTotalSpeedControl.vy;
    vy=sin_yaw*robotTotalSpeedControl.vx+cos_yaw*robotTotalSpeedControl.vy;

    //舵轮解算
    fp32 vwx=CHASSIS_R*CHASSIS_THETA_SIN*w;
    fp32 vwy=CHASSIS_R*CHASSIS_THETA_COS*w;
    AGVControl[0].vx=vx-vwx;
    AGVControl[0].vy=vy+vwy;
    AGVControl[1].vx=vx+vwx;
    AGVControl[1].vy=vy+vwy;
    AGVControl[2].vx=vx+vwx;
    AGVControl[2].vy=vy-vwy;
    AGVControl[3].vx=vx-vwx;
    AGVControl[3].vy=vy-vwy;
    for(int i=0;i<4;i++){
        if(AGVControl[i].vx>0){
            AGVControl[i].directionFlag=1;
        }else if(AGVControl[i].vx==0){
            AGVControl[i].directionFlag=0;
        }else{
            AGVControl[i].directionFlag=-1;
            AGVControl[i].vx*=-1;
            AGVControl[i].vy*=-1;
        }
    }
    for(int i=0;i<4;i++){
        if(i==1||i==2)
        {
        AGVControl[i].wantedMotorSpeed=-sqrt(AGVControl[i].vx*AGVControl[i].vx+AGVControl[i].vy*AGVControl[i].vy);
        }
        if(i==0||i==3)
        {
        if(i==1||i==2)
        {
        AGVControl[i].wantedMotorSpeed=-sqrt(AGVControl[i].vx*AGVControl[i].vx+AGVControl[i].vy*AGVControl[i].vy);
        }
        if(i==0||i==3)
        {
        AGVControl[i].wantedMotorSpeed=sqrt(AGVControl[i].vx*AGVControl[i].vx+AGVControl[i].vy*AGVControl[i].vy);
        }
        if(AGVControl[i].vx==0&&AGVControl[i].vy==0){
            AGVControl[i].wantedAngle=0;
        }else{
            AGVControl[i].wantedAngle=atan2f(AGVControl[i].vy,AGVControl[i].vx);
            while (AGVControl[i].wantedAngle>PI/2)
            {
                AGVControl[i].wantedAngle-=PI;
                AGVControl[i].directionFlag*=-1;
            }
            while (AGVControl[i].wantedAngle<-PI/2)
            {
                AGVControl[i].wantedAngle+=PI;
                AGVControl[i].directionFlag*=-1;
            }
        }
        }
        if(AGVControl[i].vx==0&&AGVControl[i].vy==0){
            AGVControl[i].wantedAngle=0;
        }else{
            AGVControl[i].wantedAngle=atan2f(AGVControl[i].vy,AGVControl[i].vx);
            while (AGVControl[i].wantedAngle>PI/2)
            {
                AGVControl[i].wantedAngle-=PI;
                AGVControl[i].directionFlag*=-1;
            }
            while (AGVControl[i].wantedAngle<-PI/2)
            {
                AGVControl[i].wantedAngle+=PI;
                AGVControl[i].directionFlag*=-1;
            }
        }
    }
}

static void firstOrderFilt()
{
    first_order_filter_cali(&robotTotalSpeedControl.vx_filter,robotTotalSpeedControl.vx);
    first_order_filter_cali(&robotTotalSpeedControl.vy_filter,robotTotalSpeedControl.vy);
    first_order_filter_cali(&robotTotalSpeedControl.w_filter,robotTotalSpeedControl.w);
    robotTotalSpeedControl.vx=robotTotalSpeedControl.vx_filter.out;
    robotTotalSpeedControl.vy=robotTotalSpeedControl.vy_filter.out;
    robotTotalSpeedControl.w=robotTotalSpeedControl.w_filter.out;
}

/**
 * @brief 将角度化为(-PI/2,PI/2)范围内
 * 
 * @param rawAngle 
 * @return fp32 
 */
fp32 radFormat(fp32 rawAngle,int8_t *flag)   //test done
{
    while(rawAngle>PI)
        rawAngle-=(2*PI);
    while(rawAngle<(-PI))
        rawAngle+=(2*PI);
    if(rawAngle>PI/2){
        rawAngle-=PI;
        *flag=*flag*(-1);
    }else if (rawAngle<(-PI/2))
    {
        rawAngle+=PI;
        *flag=*flag*(-1);
    }
    
    
    return rawAngle;
}

/**
 * @brief 计算转圈的PID。内部error先循环转化为(-PI,PI)范围
 * 
 * @param pid 
 * @param ref 反馈值
 * @param set 设定值
 * @return fp32 
 */
void GM6020_PID_calc(pid_type_def *pid, fp32 ref, fp32 set, int8_t *flag)
{
    if (pid == NULL)
    {
        pid->out=0;
    }
 
 
    pid->error[2] = pid->error[1];
    pid->error[1] = pid->error[0];
    pid->set = set;
    pid->fdb = ref;
    pid->error[0] = radFormat(set - ref,flag);
    if (pid->mode == PID_POSITION)  
    {   
        pid->Pout = pid->Kp * pid->error[0];
        pid->Iout += pid->Ki * pid->error[0];
        pid->Dbuf[2] = pid->Dbuf[1];
        pid->Dbuf[1] = pid->Dbuf[0];
        pid->Dbuf[0] = (pid->error[0] - pid->error[1]);
        pid->Dout = pid->Kd * pid->Dbuf[0];
        range_limit_inside(pid->Iout, pid->max_iout);
        pid->out = pid->Pout + pid->Iout + pid->Dout;
        range_limit_inside(pid->out, pid->max_out);
    }
    else if (pid->mode == PID_DELTA)
    {
        pid->Pout = pid->Kp * (pid->error[0] - pid->error[1]);
        pid->Iout = pid->Ki * pid->error[0];
        pid->Dbuf[2] = pid->Dbuf[1];
        pid->Dbuf[1] = pid->Dbuf[0];
        pid->Dbuf[0] = (pid->error[0] - 2.0f * pid->error[1] + pid->error[2]);
        pid->Dout = pid->Kd * pid->Dbuf[0];
        pid->out += pid->Pout + pid->Iout + pid->Dout;
        range_limit_inside(pid->out, pid->max_out);
    }
}


void calcGiveOut(void) //pid 计算电流
{
    uint8_t i;

    for(i=0;i<4;i++)
    {

         AGVControl[i].presentMotorSpeed_GM=get_chassis_motor_measure_point(i+CHASSIS_LF_G)->speed_rpm;
         GM6020_PID_calc(&(driveMotor_GM[i].vpidOfRpm),-AGVControl[i].presentAngle,AGVControl[i].wantedAngle,&(AGVControl[i].directionFlag));
         PID_calc(&(driveMotor_GM[i].vpidOfVolage),-AGVControl[i].presentMotorSpeed_GM,driveMotor_GM[i].vpidOfRpm.out);
         driveMotor_GM[i].giveCurrent=-driveMotor_GM[i].vpidOfVolage.out;
    }
    for(i=0;i<4;i++)
    {
        AGVControl[i].presentMotorSpeed=((fp32)(get_chassis_motor_measure_point(i)->speed_rpm))*WHEEL_PERIMETER/M3508_K/60;    //获取电机转速[mm/s]
        PID_calc(&(driveMotor_M[i].vpid),AGVControl[i].presentMotorSpeed,AGVControl[i].wantedMotorSpeed*AGVControl[i].directionFlag);   //pid计算
        driveMotor_M[i].giveCurrent=driveMotor_M[i].vpid.out;    //pid的输出就是驱动电流
    }
}



void chassis_task(void const *pvParameters)
{
    osDelay(CHASSIS_TASK_INIT_TIME);    //等待电机初始化
    initPIDs();                         //初始化PID
    initFilters();
    gimbalSTM_p=get_gimbal_stm_point();    //获取遥控器数据和NUC数据指针
    nuc_p=get_nuc_control_point();
    robotMode=getRobotPresentMode();

    while(1)
    {
        setChassisMode();
        analyseTotalControl();
        refreshECD();
        firstOrderFilt();
        calcWheelVelocityInChassisAxis();
        calcGiveOut();
        // powerControl(); // 2.3，功率控制
        if(zeroCurrentMark){
            CAN_cmd_chassis_M3508(0,0,0,0);       //发送零电流
            osDelay(CHASSIS_CONTROL_TIME_MS);
            CAN_cmd_chassis_GM6020(0,0,0,0);
        }
        else{
            CAN_cmd_chassis_GM6020(driveMotor_GM[0].giveCurrent,driveMotor_GM[1].giveCurrent,driveMotor_GM[2].giveCurrent,driveMotor_GM[3].giveCurrent);
            osDelay(CHASSIS_CONTROL_TIME_MS);
            CAN_cmd_chassis_M3508(driveMotor_M[0].giveCurrent,driveMotor_M[1].giveCurrent,driveMotor_M[2].giveCurrent,driveMotor_M[3].giveCurrent);   //正常控制电机
        }
    }
}

void OLED_chassis(void)
{
    // OLED_printf(4,4,"YAW_ECD:%d",get_chassis_motor_measure_point(YAW_DOWN)->ecd);
    OLED_printf(0,0,"M1:%d M2:%d M3:%d M4:%d",get_chassis_motor_measure_point(0)->speed_rpm,get_chassis_motor_measure_point(1)->speed_rpm,get_chassis_motor_measure_point(2)->speed_rpm,get_chassis_motor_measure_point(3)->speed_rpm);
    // OLED_printf(1,0,"YAW_ECD:%d M1:%d M2:%d M3:%d M4:%d",get_chassis_motor_measure_point(YAW_DOWN)->ecd,get_chassis_motor_measure_point(CHASSIS_LF_G)->ecd,get_chassis_motor_measure_point(CHASSIS_RF_G)->ecd,get_chassis_motor_measure_point(CHASSIS_RB_G)->ecd,get_chassis_motor_measure_point(CHASSIS_LB_G)->ecd);
}
